#ifndef _DWE_H
#define _DWE_H

#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <ros/ros.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <nlink_parser/LinktrackNodeframe3.h>
#include <nlink_parser/LinktrackNodeframe2.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <mavros_msgs/Altitude.h>
#include <sensor_msgs/Imu.h>
#include <string>
#include <fstream>
#include <iostream>

using namespace Eigen;
using namespace std;

class dwe{
    private:
        int dim_;

        // A and B can be a private member 
        Eigen::Matrix<double, 6, 6> A;
        Eigen::Matrix<double, 6, 3> B;
        Eigen::Matrix<double, 6, 6> AA,P,Q;
        Eigen::Matrix<double, 4, 4> R;
        // Eigen::Matrix<double, 4, 4> R;
        Eigen::Matrix<double, 1, 3> Qp,Qv,Pp,Pv;
        Eigen::Matrix<double, 1, 6> tP,tQ;
        Eigen::Matrix<double, 1, 4> tR;

    public:
        //默认构造函数
        dwe(){}
        // 构造函数
        dwe(ros::NodeHandle& nh);
        //默认析构函数
        ~dwe();

        // param in sliding window
        int delta_;
        static int lopt_;
        int k_ = 1;
        int vicon_flag_;
        double paramP_;
        double paramQ_;
        double bias_imu_;
        int uwbnum_;
        int useduwbnum_;
        double paramR_;
        double paramU_;
        double paramUz_;
        int dwe_flag_;
        // vector<double> paramUWB_;
        MatrixXd paramUWB_;
        MatrixXd paramUWB_est_;
        MatrixXd paramX0_;
        Eigen::Matrix<double, 3, 4> pos_uwb_init_;
        Vector3d pos_uav_init_;
        Vector3d pos_uwb01_init_;
        // MatrixXd R;
        // MatrixXd tR;

        int order_;
        int count_j_;
        float dt_ = 0.0;
        int LOOP_num_;
        int startNum_;
        int M_sg_size_ = 9;
        int N_sg_poly_ = 3;
        int flow_quality_;

        // int rate_;
        Eigen::Matrix<double, 1, 3> kappa_;
        Eigen::Matrix<double, 1, 3> bias_pos_;
        MatrixXd R_qua2mat;

        //ensure data input
        bool flag_init_imu_= false; 
        bool check_for_init_= false; 
        bool get_init_imu= false; 
        // bool flag_init_uwb_; 
        // bool flag_init_flow_; 
        // bool flag_init_height_; 
        bool flag_init_ok_ = false; 
        bool hasTakenOff_ = false; 
        bool hasStartEst_ = false;

        //param for camera 
        double flag_out_camA_ = 0;
        double flag_in_camA_ = 0;
        Vector3d UAV_flickering_camA_; 
        bool Is_cameraA_zeros_ = false;

        //param in estimation
        MatrixXd MHE_height_;
        MatrixXd MHE_flow_;
        MatrixXd MHE_imu_;
        MatrixXd MHE_imu_att_;
        MatrixXd MHE_uwb_;
        MatrixXd MHE_uwb_pos_;
        MatrixXd MHE_uwb_posorigin_;
        MatrixXd MHE_uwb_sensor_;

        static MatrixXd VICON_PosUAV_;
        static MatrixXd VICON_PosUWB_;
        // MatrixXd VICON_PosUWB02_;
        // MatrixXd VICON_PosUWB03_;
        // MatrixXd VICON_PosUWB04_;
        MatrixXd VICON_PosUWB_ini;
        MatrixXd VICON_VelUAV_;
        MatrixXd VICON_vel_;
        MatrixXd VICON_uwb_;
        MatrixXd VICON_uwb_ini;
        MatrixXd VICON_ImuUAV_;
        MatrixXd VICON_PosUAV2UWB01_;
        MatrixXd xt_inital_;


        static MatrixXd camA_;
        static MatrixXd camB_;
        static MatrixXd camC_;
        static MatrixXd camD_;
        static MatrixXd camE_;
        static MatrixXd camF_;
        static MatrixXd camG_;
        static MatrixXd camH_;   

        MatrixXd cameraA_vector_last;
        // MatrixXd UWB_cb_;
        // MatrixXd VICON_UAV_cb_;
        // MatrixXd VICON_UWB_cb_;
        
        double previousTime_= 0.0;
        double currentTime_= 0.0;
        MatrixXd VICON_inital_;
        MatrixXd deltaTime_;
        MatrixXd xt_;  // recommend: stl/boost list
        MatrixXd xt_real_;
        MatrixXd xt_camera;
        // MatrixXd xt_pre_;
        // MatrixXd VICON_xt_;
        MatrixXd x1_;  // initialize with nonzero numbers, e.g., x=y=z=uwb.dis/sqrt(3);

        int num_state_;
        MatrixXd Ex,Eu,W,t_matrix,ur,xtt;
        MatrixXd ar,xxt_new,xxe,Ex_new;

        // MatrixXd Ex_pre,Eu_pre,W_pre,ur_pre;
        // MatrixXd ar_pre,xxt_new_pre,xxe_pre,Ex_new_pre;

    public:
        // declaration for functions
        // void loadParameters(ros::NodeHandle& nh);

        // define the input data form uav
        // void alt_cb(mavros_msgs::Altitude msg);
        void flow_cb(const mavros_msgs::OpticalFlowRad::ConstPtr &msg);
        // void flow_cb(mavros_msgs::OpticalFlowRad msg);
        // void imu_cb(sensor_msgs::Imu msg);
        void uwb2_cb(const nlink_parser::LinktrackNodeframe2::ConstPtr &msg);
        // void uwb3_cb(nlink_parser::LinktrackNodeframe3 msg);
        void imu_new_cb(const sensor_msgs::Imu::ConstPtr &msg);

        // define the input data form vicon
        void pos_uav_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba0_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba1_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba2_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba3_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba4_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba5_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba6_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void pos_uwba7_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void vel_uav_cb(const geometry_msgs::TwistStamped::ConstPtr &msg);


        static void camA_cb(geometry_msgs::TransformStamped msg);
        static void camB_cb(geometry_msgs::TransformStamped msg);
        static void camC_cb(geometry_msgs::TransformStamped msg);
        static void camD_cb(geometry_msgs::TransformStamped msg);
        static void camE_cb(geometry_msgs::TransformStamped msg);
        static void camF_cb(geometry_msgs::TransformStamped msg);
        static void camG_cb(geometry_msgs::TransformStamped msg);
        static void camH_cb(geometry_msgs::TransformStamped msg);
        // void data_writter();
        MatrixXd qua2mat();
        MatrixXd sgolayfilt(Eigen::MatrixXd data);
        MatrixXd meanFilter(Eigen::MatrixXd data);
        MatrixXd meanFilterWithSlidingWindow(Eigen::MatrixXd data, int windowSize);
        void Initialdataxt();
        void InitialWithParam();
        void solveonce();
        void get_data();
        // void presolveonce();
        

};

#endif
